//
// Created by LiuDongPeng on 2023/11/16.
//

#include "motor.h"
#include "foc.h"

#include "main.h"
#include "tim.h"
#include "bsp.h"
#include "lowpass_filter.h"


int motor_create(motor_t *motor)
{
    if (motor == NULL)
        return -1;

    motor->id = 1;

    motor->status = MOTOR_STATUS_STOP;

    motor->udc = 24.0f;

    motor->dqVoltage.ud = 0.0f;
    motor->dqVoltage.uq = 0.0f;

    motor->pwmParam.period = 1e-4f;
    motor->pwmParam.Tpwm = 16000;

    motor->theta = 0.0f;
    motor->angleRadOffset = 3.32318878f;

    /* 电机参数 */
    motor->polePairNum = 7;
    motor->Rs = 5.8f;
    motor->Ld = 2.6f / 1000;
    motor->allElecZeroMapToMechZeroAngle[0] = 2.594592f;
    motor->allElecZeroMapToMechZeroAngle[1] = 3.32318878f;
    motor->allElecZeroMapToMechZeroAngle[2] = 4.398997f;
    motor->allElecZeroMapToMechZeroAngle[3] = 5.284354f;
    motor->allElecZeroMapToMechZeroAngle[4] = 6.200330f;
    motor->allElecZeroMapToMechZeroAngle[5] = 0.811674f;
    motor->allElecZeroMapToMechZeroAngle[6] = 1.716944f;

	/* id iq 低通滤波器初始化 */
	low_pass_filter_init(&motor->idLpFilter, 10.0f, 1e-4f);
	low_pass_filter_init(&motor->iqLpFilter, 10.0f, 1e-4f);

    /* id iq pid参数 */
//    float kp = motor->Ld * 100 * M_TWOPI;
    float kp = 6.5f;
    float ki = motor->Rs * 100 * M_TWOPI * 1e-4;
//    float ki = 0.36f;
    float upLimit = /*sqrt(3) / 3 * motor->udc*/ 8.0f;
    float lowLimit = -upLimit;
    pid_ctrl_init(&motor->dCurrentPid, 7.5f, 1.3f, 0.0f, lowLimit, upLimit);
    pid_ctrl_init(&motor->qCurrentPid, 16.3f, 1.68f, 0.0f, lowLimit, upLimit);

	/* 电机转速 低通滤波器初始化 */
	low_pass_filter_init(&motor->speedLdFilter, 10.0f, 1e-3f);

    return 0;
}

void motor_enable(motor_t* motor)
{
	if (motor == NULL)
		return;

	motor->status = MOTOR_STATUS_RUN;

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
}

void motor_disable(motor_t* motor)
{
	if (motor == NULL)
		return;

	motor->status = MOTOR_STATUS_STOP;

	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_3);
}

/**
 * @brief Set motor status
 * @param[out]  motor
 * @param[in]   status
 */
void motor_set_status(motor_t* motor, motor_status_t status)
{
    if (motor != NULL)
    {
        motor->status = status;
    }
}

/**
 * @brief Check if the motor has an encoder
 * @param motor
 * @return
 */
bool motor_has_encoder(motor_t *motor)
{
    return motor->encoder != NULL;
}

/**
 * @brief Link encoder to get motor angle
 * @param motor
 * @param encoder
 * @return
 */
bool motor_link_encoder(motor_t *motor, encoder_t* encoder)
{
    if (motor == NULL || encoder == NULL)
        return false;

    motor->encoder = encoder;
    return true;
}

/**
 * @brief Get motor angle, record tick and angle
 * @param motor
 * @return
 */
float32_t motor_get_angle_rad(motor_t *motor)
{
    if (motor != NULL && motor_has_encoder(motor))
    {
        /* 记录微秒时钟滴答和角度，用于电机转速计算 */
        motor->lastTick = bsp_get_us_tick();
        motor->lastAngle = motor->angle;

        /* 获取此刻的角度 */
        motor->encoder->encoderGetDataCallback(motor->encoder, &motor->angle, &motor->angleRad);

        return motor->angleRad;
    }
    return 0;
}

/**
 * @brief Get motor velocity in r/min
 * @param motor
 * @return
 */
float32_t motor_get_speed_rpm(motor_t* motor)
{
    float detAngle = motor->angle - motor->lastAngle;
    uint32_t tick = bsp_get_us_tick();
    uint32_t detTick = 0;
    float ts = 0;

	static bool firstHere = true;
	if (firstHere)
	{
		motor->lastAngle = motor->angle;
		motor->lastTick = tick;
		firstHere = false;

		return 0;
	}

    /* 计算间隔时间 */
    if (tick < motor->lastTick)
        detTick = UINT16_MAX - motor->lastTick + tick;
    else
        detTick = tick - motor->lastTick;
    ts = detTick * 1e-6f;

    /* 计算角度差值 */
    if (motor->angle < motor->lastAngle)
        detAngle = 360.0f - motor->lastAngle + motor->angle;

    return (detAngle / 360.0f) / ts;
}

/**
 * @brief
 * @param motor
 */
void motor_svpwm(motor_t *motor)
{
    if (motor == NULL)
        return;

    foc_svpwm(motor->alphabetaVoltage.ualpha, motor->alphabetaVoltage.ubeta, motor->udc, motor->pwmParam.Tpwm,
              &motor->pwmParam.ta, &motor->pwmParam.tb, &motor->pwmParam.tc);

    motor->pwmParam.ccra = (uint32_t) (motor->pwmParam.ta * 8000);
    motor->pwmParam.ccrb = (uint32_t) (motor->pwmParam.tb * 8000);
    motor->pwmParam.ccrc = (uint32_t) (motor->pwmParam.tc * 8000);
}

/**
 * @brief foc算法
 */
void motor_foc_algorithm_step(motor_t *motor)
{
    if (motor == NULL)
        return;

    /* 0. Get motor electricity angle */
//    motor_get_elec_angle(motor);

    /* 1. clark */
    foc_clark(motor->phaseCurrent.ia, motor->phaseCurrent.ib, motor->phaseCurrent.ic,
              &motor->alphabetaCurrent.ialpha, &motor->alphabetaCurrent.ibeta);

    /* 2. park */
    foc_park(motor->alphabetaCurrent.ialpha, motor->alphabetaCurrent.ibeta,
             motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
             &motor->dqCurrent.id, &motor->dqCurrent.iq);

    /* 3. id, iq pid ctrl */
    pid_ctrl_current_calc(&motor->dCurrentPid, 0, motor->dqCurrent.id, &motor->dqVoltage.ud);
    pid_ctrl_current_calc(&motor->qCurrentPid, 1.0f, motor->dqCurrent.iq, &motor->dqVoltage.uq);

    /* 4. inv park */
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq,
                 motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm(motor);

    /* 6. pwm output */
    motor_set_pwm_duty(motor);
}

/**
 * @brief Set 3 channel pwm duty
 * @param motor
 */
void motor_set_pwm_duty(motor_t *motor)
{
    if (motor == NULL)
        return;

    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor->pwmParam.ccra);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, motor->pwmParam.ccrb);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, motor->pwmParam.ccrc);
}

/**
 * @brief Get elec angle
 * @param motor
 * @return
 */
float motor_get_elec_angle(motor_t *motor)
{
    if (motor == NULL)
        return 0;

    motor_get_angle_rad(motor);
    if (motor->angleRad >= motor->angleRadOffset)
    {
        motor->angleRad -= motor->angleRadOffset;
    }
    else
    {
        motor->angleRad += (float)(M_TWOPI - motor->angleRadOffset);
    }

    motor->theta = motor->angleRad * (float)motor->polePairNum;

    motor->sincosTheta.sinTheta = arm_sin_f32(motor->theta);
    motor->sincosTheta.cosTheta = arm_cos_f32(motor->theta);

    return motor->theta;
}



/**
 * @brief Start motor
 * @param[in]   motor
 */
void motor_startup(motor_t *motor)
{
    if (motor == NULL)
        return;

    motor->dqVoltage.ud = 1.5f;
    motor->dqVoltage.uq = 0;
    motor->theta = 0;
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq, arm_sin_f32(0.0f), arm_cos_f32(0.0f),
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);
    motor_svpwm(motor);
    motor_set_pwm_duty(motor);

    motor->status = MOTOR_STATUS_RUN;
}


/**
 * @brief Calibrate encoder
 * @param motor
 * @param elecZeroAngleRad
 * @param polePair
 */
void motor_calib_encoder(motor_t *motor, float32_t* elecZeroAngleRad, int polePair)
{
    if (motor == NULL)
        return;

    float a, b, c, d, e, f, g;
    int index = 0;
    a = elecZeroAngleRad[index++];
    b = elecZeroAngleRad[index++];
    c = elecZeroAngleRad[index++];
    d = elecZeroAngleRad[index++];
    e = elecZeroAngleRad[index++];
    f = elecZeroAngleRad[index++];
    g = elecZeroAngleRad[index++];

    /* Get current angle */
    motor->angleRad = motor_get_angle_rad(motor);

    if (motor->angleRad > a && motor->angleRad <= b)
    {
        motor->theta = (motor->angleRad - a) / (b - a) * M_TWOPI;
    }
    if (motor->angleRad > b && motor->angleRad <= c)
    {
        motor->theta = (motor->angleRad - b) / (c - b) * M_TWOPI;
    }
    if (motor->angleRad > c && motor->angleRad <= d)
    {
        motor->theta = (motor->angleRad - c) / (d - c) * M_TWOPI;
    }
    if (motor->angleRad > d && motor->angleRad <= e)
    {
        motor->theta = (motor->angleRad - d) / (e - d) * M_TWOPI;
    }
    if (motor->angleRad > e && motor->angleRad <= f)
    {
        motor->theta = (motor->angleRad - e) / (f - e) * M_TWOPI;
    }
    if (motor->angleRad > f && motor->angleRad <= g)
    {
        motor->theta = (motor->angleRad - f) / (g - f) * M_TWOPI;
    }

    if (motor->angleRad > g && motor->angleRad <= M_TWOPI)
    {
        motor->theta = (motor->angleRad - g) / (M_TWOPI - g + a) * M_TWOPI;
    }
    if (motor->angleRad > 0 && motor->angleRad <= a)
    {
        motor->theta = (motor->angleRad - g + M_TWOPI) / (M_TWOPI - g + a) * M_TWOPI;
    }
}


/**
 * @brief Align electricity zero pos
 * @param motor
 */
void motor_align_encoder(motor_t *motor)
{
    if (motor == NULL)
        return;

	motor_enable(motor);

    motor->dqVoltage.ud = 0.3f;
    motor->dqVoltage.uq = 0;
    motor->theta = 0;
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq, arm_sin_f32(0.0f), arm_cos_f32(0.0f),
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);
    motor_svpwm(motor);
    motor_set_pwm_duty(motor);
	HAL_Delay(10);

    motor->angleRadOffset = motor_get_angle_rad(motor);

//	motor_disable(motor);
}


/**
 * @brief
 * @param motor
 * @param uq
 */
void motor_open_loop_test(motor_t *motor, float32_t uq)
{
    if (motor == NULL)
        return;

    motor->dqVoltage.ud = 0;
    motor->dqVoltage.uq = uq;

    /* 0. Get motor electricity angle */
//    motor_get_elec_angle(motor);

    /* 4. inv park */
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq, motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm(motor);

    /* 6. pwm output */
    motor_set_pwm_duty(motor);
}

/**
 * @brief Current closed loop
 * @param[in]   motor
 * @param[in]   id
 * @param[in]   iq
 */
void motor_current_closed_loop(motor_t *motor, float32_t id, float32_t iq)
{
    if (motor == NULL)
        return;

    /* 0. Get motor electricity angle */
//    motor_get_elec_angle(motor);

    /* 1. clark */
    foc_clark(motor->phaseCurrent.ia, motor->phaseCurrent.ib, motor->phaseCurrent.ic,
              &motor->alphabetaCurrent.ialpha, &motor->alphabetaCurrent.ibeta);

    /* 2. park */
    foc_park(motor->alphabetaCurrent.ialpha, motor->alphabetaCurrent.ibeta,
             motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
             &motor->dqCurrent.id, &motor->dqCurrent.iq);

    /* 3. id, iq pid ctrl */
	motor->dqCurrent.id = low_pass_filter_work(&motor->idLpFilter, motor->dqCurrent.id);
	motor->dqCurrent.iq = low_pass_filter_work(&motor->iqLpFilter, motor->dqCurrent.iq);
    pid_ctrl_current_calc(&motor->dCurrentPid, id, motor->dqCurrent.id, &motor->dqVoltage.ud);
    pid_ctrl_current_calc(&motor->qCurrentPid, iq, motor->dqCurrent.iq, &motor->dqVoltage.uq);

    /* 4. inv park */
    foc_inv_park(motor->dqVoltage.ud, motor->dqVoltage.uq,
                 motor->sincosTheta.sinTheta, motor->sincosTheta.cosTheta,
                 &motor->alphabetaVoltage.ualpha, &motor->alphabetaVoltage.ubeta);

    /* 5. svpwm */
    motor_svpwm(motor);

    /* 6. pwm output */
    motor_set_pwm_duty(motor);
}




void motor_set_pid_idkp(motor_t* motor, float32_t kp)
{
    if (motor != NULL)
    {
        if (kp < 0)
            return;
        motor->dCurrentPid.kp = kp;
    }
}
void motor_set_pid_idki(motor_t* motor, float32_t ki)
{
    if (motor != NULL)
    {
        if (ki < 0)
            return;
        motor->dCurrentPid.ki = ki;
    }
}